/*	AX-12.c & .h
 *	This module is a revised interface for the Bioloid Ax-12 servos.  It aims to be easier to use than the previous
 *	version which is in dynamixel-comms.c & .h
 *
 *	Author:  Phil Hall
 *	Date:    May 2008
 *
*/
#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
#include <avr/signal.h>
#include <string.h>
#include <stdio.h>

#include "AX-12.h"

extern void TxD80(unsigned char bTxdData);
extern volatile unsigned char gbpRxInterruptBuffer[256]; 
extern unsigned char gbpParameter[128];
extern unsigned char gbRxBufferReadPointer;
extern unsigned char gbpRxBuffer[128]; 
extern unsigned char gbpTxBuffer[128];
extern unsigned char DebugServoID; 
volatile unsigned char gbRxBufferWritePointer;
extern void TxDString(unsigned char  *bData);

extern int servo_report;

void AX12TxPacket(unsigned char *Data)
	//	Data must be a properly formed packet but not including the preamble or checksum
{
	unsigned char BytesToSend,
				    Checksum = 0;

	if (*Data == 0xFE)
	{
		CommsPrintf("Packet with Broadcast ServoID ignored:");
		AX12ReportPacket(Data);
		CommsPrintf("\r\n");
	}
	else
	{
		BytesToSend = *(Data + 1) + 1; // 2 for 'Length' & 'Instruction' then -1 for checksum
		
		RS485_TXD;
		
		sbi(UCSR0A,6);//SET_TXD0_FINISH;
		TxD80('\xff');
		sbi(UCSR0A,6);//SET_TXD0_FINISH;
		TxD80('\xff');
		
		while (BytesToSend)
		{
			Checksum += *Data;
			sbi(UCSR0A,6);//SET_TXD0_FINISH;
			TxD80(*Data);
			Data++;
			BytesToSend--;
		}
		
		sbi(UCSR0A,6);//SET_TXD0_FINISH;
		TxD80(~Checksum & '\xFF');
			
		while(!CHECK_TXD0_FINISH); //Wait until TXD Shift register empty
		
		RS485_RXD;
	}
}

int AX12RxByte(unsigned char *Byte)
{
	int		RetVal = AX12_ERR_TIMEOUT;
	
	for (unsigned long Timer = AX12_TIMEOUT; Timer > 0; Timer --)
	{
		if (gbRxBufferReadPointer != gbRxBufferWritePointer)
		{
			RetVal = 1;
			*Byte = gbpRxInterruptBuffer[gbRxBufferReadPointer++];
			break;
		}
	}
	
	return RetVal;
}

int AX12RxPacket(unsigned char *Data, int ExpectedLen, unsigned char ServoID, int BuffLen)
	//	The returned packet will not have the pre-amble or the checksum.
	//	Returns length of packet for a good read or an error code
	//	Checks the packet checksum.  Returns AX12_ERR_BAD_CHECKSUM if wrong.
	//	if ExpectedLen is non-zero then it is checked. Do not include preamble or checksum in this count.
	//	if ServoID is non-zero then it is checked.  Returns AX12_ERR_BAD_SERVO_ID if wrong.
	//	Returns AX12_ERR_BUFF_TOO_SHORT if the provided buffer is too short
	//	Does not check the status byte within the packet
{
	int				RetVal = 0;
	int				i;
	unsigned char	Rxed,
					Checksum = 0,
					BytesToDo = '\xff';

	gbRxBufferReadPointer = gbRxBufferWritePointer = 0;  //RS485 RxBuffer Clearing.
	
	//	Rx preamble
	for (i = 0; (i < 2) && (RetVal == 0); i ++)
		if (AX12RxByte(&Rxed) > 0)
		{
			if (Rxed != '\xff')
				RetVal = AX12_ERR_BAD_PREAMBLE;
		}
		else
			RetVal = AX12_ERR_TIMEOUT;
	
	//	Now rx data
	for (i = 0; (RetVal == 0); i ++, BytesToDo --)
	{
		if (AX12RxByte(&Rxed) > 0)
		{
			if (i == 1)
				BytesToDo = Rxed;
			
			if (i < BuffLen)
				*(Data + i) = Rxed;
			else
				RetVal = AX12_ERR_BUFF_TOO_SHORT;
			
			if (BytesToDo >= 1)
				Checksum += Rxed;
				
			if (BytesToDo == 0)
				break;
		}
		else
			RetVal = AX12_ERR_TIMEOUT;
	}

	if (RetVal == 0)
		if ((Checksum & '\xff') != (~Rxed & '\xff'))
			RetVal = AX12_ERR_BAD_CHECKSUM;
		
	if (ServoID && (RetVal == 0))
		if (ServoID != *Data)
			RetVal = AX12_ERR_BAD_SERVO_ID;
		
	if (ExpectedLen && (RetVal == 0))
		if (ExpectedLen != (i+1))
			RetVal = AX12_ERR_BAD_PACKET_LEN;
			
	if (RetVal == 0)
		RetVal = i;
		
	if (ServoID == DebugServoID)
		AX12ReportPacket(Data);
			
	return RetVal;
}


void AX12ReportPacket(unsigned char *Packet)
{
	int		i, j, PacketLen;
	char	Buffer[200];
	
	PacketLen = *(Packet+1)+2;	//	2: ID & Len

	sprintf (Buffer, "[");
	
	for (i = 0, j = 1; i < PacketLen; i ++, j += 3)
		sprintf(&Buffer[j], "%02X ", *(Packet+i));

	sprintf (&Buffer[j], "]");
	TxDString((unsigned char *)Buffer);
}

int AX12CheckStatus(unsigned char *Packet)
	//	Checks the Status byte in the given packet
{
	int		RetVal = 0;
	unsigned char	ErrorByte;
	
	ErrorByte = *(Packet + 2);
	
	if (ErrorByte)
		RetVal = AX12_SERVO_ERROR;
	
	return RetVal;
}


int AX12ReadMemory(unsigned char ServoID, unsigned char StartAddress, unsigned char BytesToRead, unsigned char *Data)
{
	unsigned char	ServoPacket [AX12_MAX_PACKET_LEN];
	int				RetVal = 0;
	
	ServoPacket[0] = ServoID;
	ServoPacket[1] = 4;			//	Length = no of bytes following this field (don't forget checksum)
	ServoPacket[2] = AX12_READ;
	ServoPacket[3] = StartAddress;
	ServoPacket[4] = BytesToRead;

	AX12TxPacket(ServoPacket);
	
	RetVal = AX12RxPacket(ServoPacket, BytesToRead+4, ServoID, AX12_MAX_PACKET_LEN);
	
	if (RetVal > 0)
		memcpy(Data, &ServoPacket[3], BytesToRead);
	
	return RetVal;
}

int AX12WriteMemory(unsigned char ServoID, unsigned char StartAddress, unsigned char BytesToWrite, unsigned char *Data)
{
	unsigned char	ServoPacket [AX12_MAX_PACKET_LEN];
	int				RetVal = 0;
	
	ServoPacket[0] = ServoID;
	ServoPacket[1] = BytesToWrite + 3;	//	Length.  3 = AX12_WRITE + StartAddress + Checksum
	ServoPacket[2] = AX12_WRITE;
	ServoPacket[3] = StartAddress;
	
	memcpy(&ServoPacket[4], Data, BytesToWrite);
	
	AX12TxPacket(ServoPacket);
	
	RetVal = AX12RxPacket(ServoPacket, 4, ServoID, AX12_MAX_PACKET_LEN);
	
	if (RetVal > 0)
		RetVal = AX12CheckStatus(ServoPacket);
		
	return RetVal;
}

int AX12WriteUShort(unsigned char ServoID, unsigned char StartAddress, unsigned short ushort)
{
	unsigned char	cData[10];
	
	cData[0] = ushort & 0xFF;
	cData[1] = (ushort >> 8) & 0xff;
	
	return AX12WriteMemory(ServoID, StartAddress, 2, cData);
}


int AX12ReadUShort(unsigned char ServoID, unsigned char StartAddress, unsigned short *ushort)
	//	Reads an unsigned short from StartAddress & StartAddress+1
{
	int	RetVal;
	unsigned char cData [10];

	RetVal = AX12ReadMemory(ServoID, StartAddress, 2, cData);
	
	if (RetVal > 0)
		*ushort = (cData[1] << 8) + cData[0];
	
	return RetVal;
}


int AX12GetPosition(unsigned char ServoID, unsigned short *Position)
	//	Gets the current position of the specified servo
{
	return AX12ReadUShort(ServoID, AX12_PRESENT_POSITION_L, Position);
}


int AX12SetPosition(unsigned char ServoID, unsigned short Position, unsigned short Speed)
	//	Sets the target position and speed of the specified servo
{
	int				RetVal = AX12_ERR_OUT_OF_BOUNDS;
	unsigned char	cData[10];
	
	AX12SetBounds(&Position,	AX12_MIN_POSITION,	AX12_MAX_POSITION);
	AX12SetBounds(&Speed,		AX12_MIN_SPEED, 	AX12_MAX_SPEED);

	cData[0] =  Position & 0xFF;
	cData[1] = (Position >> 8) & 0xFF;
	cData[2] =  Speed    & 0xFF;
	cData[3] = (Speed    >> 8) & 0xFF;
	
	RetVal = AX12WriteMemory(ServoID, AX12_GOAL_POSITION_L, 4, cData);
	if (servo_report == 2){
		unsigned char tposition[2];
		AX12GetPosition(ServoID,tposition);
		CommsPrintf("%d \r\n",(int)tposition);
	}
	return RetVal;
}

int AX12SetMode(unsigned char ServoID, int Mode)
		//	Mode != 1:	Servo seeks goal position
		//	Mode == 1:	Free rotating mode
{
	unsigned char	cData[10];
	cData[0] = 0;
	cData[1] = 0;
	cData[2] = (Mode == 1)? 0: 0xFF;
	cData[3] = (Mode == 1)? 0: 0x03;
	return AX12WriteMemory(ServoID, AX12_CW_ANGLE_LIMIT_L, 4, cData);
}

int AX12SetFreeSpinSpeed(unsigned char ServoID, short Speed)
		//	Sets the speed when in free spin mode
		//	Speed should be in the range -511 to +511. -ve implies anti-clockwise rotation
{
	unsigned short	uSpeed = (Speed < 0)? (unsigned short)(Speed * -1): (unsigned short)Speed;	
	if (uSpeed > 511)	uSpeed = 511;
	if (Speed > 0)		uSpeed |= 0x400;
	return AX12WriteUShort(ServoID, AX12_GOAL_SPEED_L, uSpeed);
}

int AX12Ping(unsigned char ServoID)
{
	unsigned char	ServoPacket [AX12_MAX_PACKET_LEN];
	int		RetVal = 0;
	
	ServoPacket[0] = ServoID;
	ServoPacket[1] = 2;				//	Length: AX12_PING + Checksum
	ServoPacket[2] = AX12_PING;
	
	AX12TxPacket(ServoPacket);
	
	RetVal = AX12RxPacket(ServoPacket, 4, ServoID, AX12_MAX_PACKET_LEN);
	
	if (RetVal > 0)
		RetVal = AX12CheckStatus(ServoPacket);
		
	return RetVal;
}

int AX12SetBounds(unsigned short *Val, unsigned short MinVal, unsigned short MaxVal)
	//	Forces *Val to be within MinVal & MaxVal.  Returns non-zero if was already within bounds
{
	int		RetVal = 1;
	
	if (*Val < MinVal)
	{
		RetVal = 0;
		*Val = MinVal;
	}
	
	if (*Val > MaxVal)
	{
		RetVal = 0;
		*Val = MaxVal;
	}
	
	return RetVal;
}

void CommsPrintf(char *Format, ...)
{
	extern int	vsprintf(char *__s, const char *__fmt, va_list ap);
	va_list		ap;
	char		Buff1[512];
	
	va_start(ap, Format);
	vsprintf(Buff1, Format, ap);
	va_end(ap);
	
	TxDString((unsigned char *)Buff1);
}

